from typing import Dict, Any
from intelli_port.data_layer.clients import mysql_execute_write
from intelli_port.notifications.notifier import broadcast_robot_status


from intelli_port.commons.config import load_settings


class Ros2Nav2Executor:
    def __init__(self):
        self._ready = False
        try:
            import rclpy
            from rclpy.action import ActionClient
            from nav2_msgs.action import NavigateToPose
            from geometry_msgs.msg import PoseStamped
            self.rclpy = rclpy
            self.ActionClient = ActionClient
            self.NavigateToPose = NavigateToPose
            self.PoseStamped = PoseStamped
            self._node = None
            self._ready = True
        except Exception:
            self._ready = False
        self._settings = load_settings()

    def _ensure_node(self):
        if self._node is not None:
            return True
        if not self._ready:
            return False
        try:
            self.rclpy.init(args=None)
            from rclpy.node import Node
            self._node = Node('intelli_port_exec')
            ns_prefix = str(getattr(self._settings, 'robot_namespace_prefix', 'robot') or 'robot')
            self._client = None
            self._ns_prefix = ns_prefix
            return True
        except Exception:
            return False

    def navigate_to(self, robot_id: int, target: Dict[str, Any]) -> Dict[str, Any]:
        if not self._ensure_node():
            return {"ok": False}
        try:
            pose = self.PoseStamped()
            pose.header.frame_id = str(getattr(self._settings, 'robot_nav_frame', 'map') or 'map')
            x = float(target.get('goal_x') or 0.0)
            y = float(target.get('goal_y') or 0.0)
            yaw = float(target.get('yaw') or 0.0)
            pose.pose.position.x = x
            pose.pose.position.y = y
            import math
            qz = math.sin(yaw * 0.5)
            qw = math.cos(yaw * 0.5)
            pose.pose.orientation.z = qz
            pose.pose.orientation.w = qw
            goal_msg = self.NavigateToPose.Goal()
            goal_msg.pose = pose
            topic = 'navigate_to_pose'
            mapping = str(getattr(self._settings, 'robot_namespace_map', '') or '')
            ns_map = {}
            if mapping:
                try:
                    for pair in mapping.split(','):
                        if ':' in pair:
                            k, v = pair.split(':', 1)
                            ns_map[int(k.strip())] = v.strip()
                except Exception:
                    ns_map = {}
            ns_db = None
            try:
                from intelli_port.data_layer.clients import mysql_execute_read
                rr = mysql_execute_read("SELECT namespace FROM robots WHERE id=%s", (int(robot_id),), fetch="one").get("rows", [])
                if rr and rr[0] and rr[0][0] is not None:
                    ns_db = str(rr[0][0])
            except Exception:
                ns_db = None
            ns = ns_db or ns_map.get(int(robot_id)) or str(target.get('namespace') or f"{self._ns_prefix}_{robot_id}")
            full = f"/{ns}/{topic}"
            try:
                self._client = self.ActionClient(self._node, self.NavigateToPose, full)
            except Exception:
                self._client = self.ActionClient(self._node, self.NavigateToPose, topic)
            if not self._client.wait_for_server(timeout_sec=0.5):
                return {"ok": False}
            goal_future = self._client.send_goal_async(goal_msg)

            def _goal_response_cb(fut):
                try:
                    goal_handle = fut.result()
                    tid = int(target.get('task_id') or 0)
                    if not goal_handle.accepted:
                        if tid > 0:
                            try:
                                mysql_execute_write(
                                    "UPDATE robot_tasks SET status=%s, updated_at=NOW() WHERE id=%s AND robot_id=%s",
                                    ("failed", tid, robot_id),
                                )
                            except Exception:
                                pass
                        try:
                            broadcast_robot_status(robot_id, {"task_id": tid, "status": "failed"})
                            try:
                                mysql_execute_write(
                                    "INSERT INTO robot_events(robot_id, task_id, event, detail, created_at) VALUES(%s,%s,%s,%s,NOW())",
                                    (robot_id, tid, "failed", ""),
                                )
                            except Exception:
                                pass
                        except Exception:
                            pass
                        return
                    result_future = goal_handle.get_result_async()

                    def _result_cb(rf):
                        try:
                            _ = rf.result()
                            if tid > 0:
                                try:
                                    mysql_execute_write(
                                        "UPDATE robot_tasks SET status=%s, updated_at=NOW() WHERE id=%s AND robot_id=%s",
                                        ("done", tid, robot_id),
                                    )
                                except Exception:
                                    pass
                            try:
                                broadcast_robot_status(robot_id, {"task_id": tid, "status": "done"})
                                try:
                                    mysql_execute_write(
                                        "INSERT INTO robot_events(robot_id, task_id, event, detail, created_at) VALUES(%s,%s,%s,%s,NOW())",
                                        (robot_id, tid, "done", ""),
                                    )
                                except Exception:
                                    pass
                            except Exception:
                                pass
                        except Exception:
                            if tid > 0:
                                try:
                                    mysql_execute_write(
                                        "UPDATE robot_tasks SET status=%s, updated_at=NOW() WHERE id=%s AND robot_id=%s",
                                        ("failed", tid, robot_id),
                                    )
                                except Exception:
                                    pass
                            try:
                                broadcast_robot_status(robot_id, {"task_id": tid, "status": "failed"})
                                try:
                                    mysql_execute_write(
                                        "INSERT INTO robot_events(robot_id, task_id, event, detail, created_at) VALUES(%s,%s,%s,%s,NOW())",
                                        (robot_id, tid, "failed", ""),
                                    )
                                except Exception:
                                    pass
                            except Exception:
                                pass

                    try:
                        result_future.add_done_callback(_result_cb)
                    except Exception:
                        pass
                except Exception:
                    pass

            try:
                goal_future.add_done_callback(_goal_response_cb)
            except Exception:
                pass
            return {"ok": True, "task_id": int(target.get('task_id') or 0)}
        except Exception:
            return {"ok": False}

    def cancel(self, robot_id: int, task_id: int) -> Dict[str, Any]:
        if not self._ensure_node():
            return {"ok": False}
        try:
            try:
                mysql_execute_write(
                    "UPDATE robot_tasks SET status=%s, updated_at=NOW() WHERE id=%s AND robot_id=%s",
                    ("cancelled", int(task_id), int(robot_id)),
                )
            except Exception:
                pass
            try:
                broadcast_robot_status(robot_id, {"task_id": int(task_id), "status": "cancelled"})
                try:
                    mysql_execute_write(
                        "INSERT INTO robot_events(robot_id, task_id, event, detail, created_at) VALUES(%s,%s,%s,%s,NOW())",
                        (robot_id, int(task_id), "cancelled", ""),
                    )
                except Exception:
                    pass
            except Exception:
                pass
            return {"ok": True}
        except Exception:
            return {"ok": False}

    def status(self, robot_id: int) -> Dict[str, Any]:
        ok = self._ensure_node()
        return {"ok": ok, "status": "moving" if ok else "unknown"}